
#include "rviz_tools_pkg/add_point_tool.hpp"
#include "rviz_common/display_context.hpp"

namespace rviz_tools_pkg
{
    AddPointTool::AddPointTool() : rviz_default_plugins::tools::PoseTool(),
                                   logger_(rclcpp::get_logger("add_point_tool_node"))
    {
        node_ = std::make_shared<rclcpp::Node>("add_point_tool_node");
        pose_pub_ = node_->create_publisher<geometry_msgs::msg::PoseStamped>(
            "/rviz_tools_pkg/add_point",
            10);
    }

    AddPointTool::~AddPointTool()
    {
        pose_pub_.reset();
    }

    void AddPointTool::onInitialize()
    {
        rviz_default_plugins::tools::PoseTool::onInitialize();
        rviz_common::Tool::setName("Add Point");
    }

    void AddPointTool::onPoseSet(double x, double y, double theta)
    {
        RCLCPP_INFO(logger_, "onPoseSet 响应 x = %.2f y=%.2f theta= %.2f", x, y, theta);
        // 点击位置
        geometry_msgs::msg::Pose new_pos;
        new_pos.position.x = x;
        new_pos.position.y = y;
        new_pos.position.z = 0.0;
        // 点位
        geometry_msgs::msg::PoseStamped poseStamped;
        poseStamped.header.frame_id = context_->getFixedFrame().toStdString();
        poseStamped.pose = new_pos;
        pose_pub_->publish(poseStamped);
    }
} // namespace rviz_tools_pkg

#include <pluginlib/class_list_macros.hpp>
PLUGINLIB_EXPORT_CLASS(rviz_tools_pkg::AddPointTool, rviz_common::Tool)
